
% 
% %--------------------------------------------------------------------------
% init_vector = [];
% interval=[0 T];
% [t,y] = ode45(@(t,y) altitude_plant(t,y,type),interval,init_vector);


clc; clear; close all;

% 初始条件
phi_0 = [-0.1; 0.2; 0.2]; % 初始姿态 (rad)
phi_dot_0 = [0; 0; 0]; % 初始角速度 (rad/s)
xf = [0; 0; 0]; 
xf_dot = [0; 0; 0]; 
gamma_hat_0 = 1; % 初始自适应参数
k_hat_0 = 1;
% 初始状态向量
x0 = [phi_0; phi_dot_0; xf; xf_dot; gamma_hat_0; k_hat_0];

% 控制参数
T_sigma1 = 1; T_sigma2 = 1; T_s = 4; T_lambda = 4;
sigma1 = 5/3; sigma2 = 3/2; sigma3 = 37/39; sigma4 = 39/37;
lambda1 = 1/2; lambda2 = 3/2;
pi_0 = 0.1; eta_d = 0.9; eta_lambda = 0.9;
cd = 84; c_lambda = 3;  g = 3/2; 

alpha_s2 = 1/( eta_lambda * T_lambda * (1-lambda1) * 2^(lambda1/2-3/2) );
beta_s2 = (2^(lambda2/2+3/2)) / ( eta_lambda * T_lambda * (lambda2 - 1) );

alpha_d2 = 1/( eta_d * T_sigma2 * (1-sigma3) * 2^(sigma3/2-3/2) );
beta_d2 = (2^(sigma4/2+3/2)) / ( eta_d * T_sigma2 * (sigma4 - 1) );

% 仿真时间
T_sim = 10; % 仿真时间 (s)

%% 使用 ode45 求解
[t, x] = ode45(@(t, x) altitude_plant(t, x), [0, T_sim], x0);

%% 提取结果
phi = x(:, 1:3)'; % 姿态角
phi_dot = x(:, 4:6)'; % 姿态角速度
xf = x(:,7:9)';
xf_dot = x(:,10:12)';
gamma_hat = x(:, 13)'; % 自适应参数
k_hat = x(:, 14)';

%% 计算输出



%% 绘图
figure(1);
% subplot(3, 1, 1);
plot(t, phi);
title('姿态角 (phi)');
xlabel('时间 (s)');
ylabel('角度 (rad)');
legend('\phi_1', '\phi_2', '\phi_3');

figure(2);
% subplot(3, 1, 1);
plot(t, gamma_hat);
title('姿态角 (phi)');
xlabel('时间 (s)');
ylabel('角度 (rad)');
legend('gamma_hat');

figure(3);
% subplot(3, 1, 1);
plot(t, k_hat);
title('姿态角 (phi)');
xlabel('时间 (s)');
ylabel('角度 (rad)');
legend('k_hat');



% subplot(3, 1, 2);
% plot(t, tau_hat);
% title('干扰估计 (tau\_hat)');
% xlabel('时间 (s)');
% ylabel('估计值 (N·m)');
% legend('\tau_{hat,1}', '\tau_{hat,2}', '\tau_{hat,3}');
% 
% subplot(3, 1, 3);
% plot(t, gamma_hat);
% title('自适应参数 (gamma\_hat)');
% xlabel('时间 (s)');
% ylabel('参数值');
% legend('\gamma_{hat}');
